/*
 * ppmGen.cpp
 *
 *  Created on: 16.04.2013
 *      Author: Jonas
 *	letzes Update:
		Author: Jonas | Zweck: Source-Code aufgeräumt
 */

#include "ppmGen.h"

/*
 * Initialisieren des PPM-Generators
 */
void ppmGenInit() {
	//Datenrichtung einstellen
	DDR_MOTOR_1 |= ( 1 << PIN_MOTOR_1 );
	DDR_MOTOR_2 |= ( 1 << PIN_MOTOR_2 );
	DDR_MOTOR_3 |= ( 1 << PIN_MOTOR_3 );
	DDR_MOTOR_4 |= ( 1 << PIN_MOTOR_4 );

	//Alle Pins ausschalten
	PORT_MOTOR_1 &= ~( 1 << PIN_MOTOR_1 );
	PORT_MOTOR_2 &= ~( 1 << PIN_MOTOR_2 );
	PORT_MOTOR_3 &= ~( 1 << PIN_MOTOR_3 );
	PORT_MOTOR_4 &= ~( 1 << PIN_MOTOR_4 );
	
	//0 senden, um Regler zu kalibrieren
	for ( uint8_t i = 0 ; i < 100 ; i++ ) {
		//5 wird als 0 festgelegt
		ppmSendSignal( 5 , 5 , 5 , 5);
		//kurz warten
		_delay_ms(15);
	}
}

/* 
 * Motorwerte an Regler senden
 */
void ppmSendSignal( int8_t power_motor1, int8_t power_motor2, int8_t power_motor3, int8_t power_motor4 ) {
	//Werte sortieren
	int8_t werte[4] = { power_motor1, power_motor2, power_motor3, power_motor4 };
	//stellt Motorschaltreihenfolge dar:
	uint8_t reihenfolge[4] = { 1, 2, 3, 4 };
	
	// negative werte abfangen
	for ( uint8_t i = 0; i < 3 ; i++ ){
		if ( werte[i] < 0 ){
			werte[i] = 0;
		}
	}
	
	// Falls Motorschalter- aus:
	if ( empf_wert[4] < 20 ){
		werte[0] = 0;
		werte[1] = 0;
		werte[2] = 0;
		werte[3] = 0;
	}
	//Werte in Debug-Array schreiben, zur Auswertung am Rechner (falls Debug erlaubt)
	#ifdef DEBUG_ENABLED
	send_buffer[8] = werte[0];
	send_buffer[9] = werte[1];
	send_buffer[10] = werte[2];
	send_buffer[11] = werte[3];
	#endif
	
	//Werte sortieren
	while( 1 ) {
		uint8_t tausch = 0;
		for(uint8_t i = 0; i < 3; i++) {
			if( werte[i] > werte[i+1] ) {
				//vertauschen, wenn links ein niedrigerer Wert steht
				uint8_t temp = werte[i];
				werte[i] = werte[i+1];
				werte[i+1] = temp;
				//Reihenfolge angleichen
				temp = reihenfolge[i];
				reihenfolge[i] = reihenfolge[i+1];
				reihenfolge[i+1] = temp;
				//Tauschvariable erhöhen
				tausch++;
			}
		}

		//abbrechen, wenn nicht vertauscht werden musste
		//da dann das Array fertig sortiert ist
		if( tausch == 0 ) {
			break;
		}
	}

	//Werte entsprechend Zeit berechnen
	//Zeit berechnen
	for( uint8_t i = 0; i < 4; i++ ) {
		// /10 weil schleife in 10 us
		werte[i] = werte[i] * US_PRO_PROZENT;
	}

	//von jedem Wert Vorgängerwerte abziehen:
	//(sonst stimmen Zeitwerte später nicht)
	for( uint8_t i = 0; i < 4; i++ ) {
		uint8_t counter = i;
		while( counter > 0 ) {
			counter--;
			werte[i] -= werte[counter];
		}
	}

	//Werte senden
	//Alle Ports einschalten:
	PORT_MOTOR_1 |= ( 1 << PIN_MOTOR_1 );
	PORT_MOTOR_2 |= ( 1 << PIN_MOTOR_2 );
	PORT_MOTOR_3 |= ( 1 << PIN_MOTOR_3 );
	PORT_MOTOR_4 |= ( 1 << PIN_MOTOR_4 );


	// 0% entspricht 1 ms --->>>>> OFFSET
	_delay_us(10 * MINPOWER );


	//Zeitschleifen
	for( uint8_t i = 0; i < 4; i++ ) {
		//Zeit abwarten
		for( uint8_t k = 0; k < werte[i]; k++ ) {
			_delay_us(10);
		}
		//Abfrage welcher Motor ausgeschaltet werden muss
		switch( reihenfolge[i] ) {
			case 1:
				PORT_MOTOR_1 &= ~( 1 << PIN_MOTOR_1 );
				break;
			case 2:
				PORT_MOTOR_2 &= ~( 1 << PIN_MOTOR_2 );
				break;
			case 3:
				PORT_MOTOR_3 &= ~( 1 << PIN_MOTOR_3 );
				break;
			case 4:
				PORT_MOTOR_4 &= ~( 1 << PIN_MOTOR_4 );
				break;
			default:
				break;
		}
	}
}


